#!/usr/bin/env python3
import math
import time
from enum import Enum
from typing import Optional, List, Dict, Any

import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.time import Time
from rcl_interfaces.msg import ParameterDescriptor, ParameterType

from geometry_msgs.msg import Twist, Point, Pose2D
from nav_msgs.msg import Odometry
from std_srvs.srv import Trigger, SetBool
from visualization_msgs.msg import Marker

from ball_collector_interfaces.msg import BallInfo, RobotState as RobotStateMsg
from ball_collector_interfaces.srv import ChangeColor, SetGripper,DetectTargetZone

class RobotStateEnum(Enum):
    """
    机器人状态枚举
    """
    IDLE = 'IDLE'               # 空闲状态
    SEARCHING = 'SEARCHING'     # 搜索球
    APPROACHING = 'APPROACHING' # 接近球
    ALIGNING = 'ALIGNING'       # 对准球
    GRASPING = 'GRASPING'       # 抓取球
    TRANSPORTING = 'TRANSPORTING' # 运输球
    RELEASING = 'RELEASING'     # 释放球
    RETURNING = 'RETURNING'     # 返回搜索位置
    ERROR = 'ERROR'             # 错误状态

class StrategyNode(Node):
    """
    球收集策略节点，负责机器人的高级行为控制
    """
    def __init__(self):
        super().__init__('strategy_node')
        
        # 创建互斥回调组
        # self.timer_cb_group = MutuallyExclusiveCallbackGroup()
        # 在 __init__ 中修改
        self.state_timer_cb_group = ReentrantCallbackGroup()
        self.status_timer_cb_group = ReentrantCallbackGroup()
        self.service_cb_group = ReentrantCallbackGroup()
        
        # 声明参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('target_color', 'red',
                 ParameterDescriptor(type=ParameterType.PARAMETER_STRING,
                                     description='Target ball color')),
                ('search_angular_speed', 0.5,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Angular speed when searching (rad/s)')),
                ('approach_linear_speed', 0.2,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Linear speed when approaching (m/s)')),
                ('approach_distance', 0.3,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Distance to approach the ball (m)')),
                ('center_tolerance', 30,
                 ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER,
                                     description='Tolerance for ball centering (pixels)')),
                ('target_zone_x', 1.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='X coordinate of target zone (m)')),
                ('target_zone_y', 1.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Y coordinate of target zone (m)')),
                ('camera_width', 640,
                 ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER,
                                     description='Camera width in pixels')),
                ('camera_height', 480,
                 ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER,
                                     description='Camera height in pixels')),
                ('ball_lost_timeout', 2.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Time before considering ball as lost (s)')),
                ('max_operation_time', 300.0,
                 ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE,
                                     description='Maximum operation time (s)'))
            ]
        )
        
        # 获取参数
        self.target_color = self.get_parameter('target_color').value
        self.search_angular_speed = self.get_parameter('search_angular_speed').value
        self.approach_linear_speed = self.get_parameter('approach_linear_speed').value
        self.approach_distance = self.get_parameter('approach_distance').value
        self.center_tolerance = self.get_parameter('center_tolerance').value
        self.target_zone_x = self.get_parameter('target_zone_x').value
        self.target_zone_y = self.get_parameter('target_zone_y').value
        self.camera_width = self.get_parameter('camera_width').value
        self.camera_height = self.get_parameter('camera_height').value
        self.ball_lost_timeout = self.get_parameter('ball_lost_timeout').value
        self.max_operation_time = self.get_parameter('max_operation_time').value
        
        # 状态和计时变量
        self.state = RobotStateEnum.IDLE
        self.prev_state = None
        self.current_ball = None
        self.last_ball_time = 0.0
        self.operation_start_time = 0.0
        self.has_ball = False
        self.total_collected = 0
        self.robot_pose = Pose2D()
        self.start_pose = Pose2D()
        
        # 创建订阅
        self.ball_sub = self.create_subscription(
            BallInfo,
            'detected_ball',
            self.ball_callback,
            10
        )
        
        self.odom_sub = self.create_subscription(
            Odometry,
            'odom',
            self.odom_callback,
            10
        )
        
        # 创建发布者
        self.cmd_vel_pub = self.create_publisher(
            Twist,
            'cmd_vel',
            10
        )
        
        self.state_pub = self.create_publisher(
            RobotStateMsg,
            'robot_state',
            10
        )
        
        # self.target_marker_pub = self.create_publisher(
        #     Marker,
        #     'target_zone',
        #     1
        # )
        
        # 创建客户端
        # 使用等待超时来确保服务存在
        self.gripper_client = self.create_client(
            SetGripper,
            '/hardware_interface/set_gripper',
            callback_group=self.service_cb_group
        )
        
        self.change_color_client = self.create_client(
            ChangeColor,
            'change_target_color',
            callback_group=self.service_cb_group
        )
        self.target_detection_client = self.create_client(
            DetectTargetZone,
            'detect_target_zone',
            callback_group=self.service_cb_group
        )
        
        # 等待服务可用
        self.wait_for_services()
        self.state_timer = self.create_timer(
            1,  # 10Hz
            self.state_machine_callback,
            callback_group=self.state_timer_cb_group
        )

        self.status_timer = self.create_timer(
            1.0,  # 1Hz
            self.publish_status,
            callback_group=self.status_timer_cb_group
        )
        
        # 初始化操作
        # self.publish_target_zone_marker()
        self.operation_start_time = self.get_clock().now().seconds_nanoseconds()[0]
        
        self.get_logger().info("策略节点已初始化，等待命令...")
    
    def wait_for_services(self):
        """等待所需服务可用"""
        max_tries = 5
        services = [
            ('gripper_client', self.gripper_client),
            ('change_color_client', self.change_color_client),
            ('detect_target_zone', self.target_detection_client)    
        ]
        
        for name, client in services:
            tries = 0
            while not client.wait_for_service(timeout_sec=1.0):
                tries += 1
                if tries >= max_tries:
                    self.get_logger().warn(f"服务 {name} 在{max_tries}次尝试后仍不可用")
                    break
                self.get_logger().info(f"等待服务 {name}... ({tries}/{max_tries})")
    
    def start(self):
        """开始收集操作"""
        self.get_logger().info("开始收集操作")
        self.operation_start_time = self.get_clock().now().seconds_nanoseconds()[0]
        self.get_logger().info("时间")

        self.start_pose = self.robot_pose
        self.get_logger().info("sj")
        self.total_collected = 0
   
        
        # 打开夹爪
        self.set_gripper(False)
        self.get_logger().info("griper")
        
        # 切换到搜索状态
        self.transition_to(RobotStateEnum.SEARCHING)
        self.get_logger().info("trans")

    
    def stop(self):
        """停止收集操作"""
        self.get_logger().info("停止收集操作")
        self.publish_cmd_vel(0.0, 0.0)  # 停止运动
        self.transition_to(RobotStateEnum.IDLE)
    
    def odom_callback(self, msg):
        """里程计回调，更新机器人位置"""
        self.robot_pose.x = msg.pose.pose.position.x
        self.robot_pose.y = msg.pose.pose.position.y
        
        # 从四元数提取偏航角(简化处理)
        q = msg.pose.pose.orientation
        self.robot_pose.theta = math.atan2(2.0 * (q.w * q.z + q.x * q.y),
                                          1.0 - 2.0 * (q.y * q.y + q.z * q.z))
        self.get_logger().debug("完成更新机器人位置")
    
    def ball_callback(self, msg):
        """球检测回调"""
        self.get_logger().debug(f"接收到球检测消息: 颜色={msg.color}, 是否目标={msg.is_target}")
        try:
            # 仅关注目标颜色的球
            if msg.color == self.target_color and msg.is_target:
                self.get_logger().debug("开始更新当前球信息")
                self.current_ball = msg
                self.last_ball_time = self.get_clock().now().seconds_nanoseconds()[0]
                self.get_logger().info(f"更新当前球信息: 颜色={msg.color}, 位置=({msg.center.x}, {msg.center.y})")
                self.get_logger().debug("完成更新当前球信息")
        except Exception as e:
            self.get_logger().error(f"处理球体检测消息时出错: {str(e)}")
    
    def publish_status(self):
        """发布状态信息"""
        msg = RobotStateMsg()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.current_state = self.state.value
        msg.target_color = self.target_color
        msg.has_ball = self.has_ball
        msg.robot_pose = self.robot_pose
        
        # 计算电池百分比(模拟)
        elapsed_time = self.get_clock().now().seconds_nanoseconds()[0] - self.operation_start_time
        battery_percent = max(0, 100.0 - (elapsed_time / self.max_operation_time) * 100.0)
        msg.battery_percentage = battery_percent
        
        self.state_pub.publish(msg)
    
    def publish_cmd_vel(self, linear_x, angular_z):
        """发布速度命令"""
        cmd = Twist()
        cmd.linear.x = linear_x
        cmd.angular.z = angular_z
        self.cmd_vel_pub.publish(cmd)
    
    def transition_to(self, new_state):
        """状态转换"""
        if new_state != self.state:
            self.get_logger().info(f"状态转换: {self.state.value} -> {new_state.value}")
            self.prev_state = self.state
            self.state = new_state
    
    def state_machine_callback(self):
        """状态机回调 - 增强版"""
        try:
            # 检查操作时间是否超时
            current_time = self.get_clock().now().seconds_nanoseconds()[0]
            elapsed_time = current_time - self.operation_start_time
            
            # 设置最大重试次数和延时
            if not hasattr(self, 'retry_count'):
                self.retry_count = 0
                self.max_retries = 3
                self.retry_delay = 2.0  # 秒
                self.last_retry_time = 0.0
            
            # 检查硬件和传感器状态
            hardware_ok = self.check_hardware_status()
            sensors_ok = self.check_sensor_status()
            
            # 如果硬件或传感器有问题，尝试恢复
            if not hardware_ok or not sensors_ok:
                # 避免频繁重试
                if current_time - self.last_retry_time > self.retry_delay:
                    self.retry_count += 1
                    self.last_retry_time = current_time
                    
                    if self.retry_count <= self.max_retries:
                        self.get_logger().warn(f"检测到系统异常，尝试恢复 (尝试 {self.retry_count}/{self.max_retries})")
                        self.try_recover_system()
                        return
                    else:
                        self.get_logger().error(f"系统恢复失败，超过最大尝试次数 ({self.max_retries})")
                        self.transition_to(RobotStateEnum.ERROR)
                        return
            else:
                # 系统正常，重置重试计数
                self.retry_count = 0
            
            if elapsed_time > self.max_operation_time and self.state != RobotStateEnum.IDLE:
                self.get_logger().warn(f"操作超时 ({elapsed_time:.1f}s > {self.max_operation_time:.1f}s)")
                self.stop()
                return
            self.get_logger().debug(f"当前状态: {self.state.value}")
            # 状态机逻辑 - 使用更健壮的处理方式
            if self.state == RobotStateEnum.IDLE:
                # 空闲状态，停止所有运动
                self.publish_cmd_vel(0.0, 0.0)
            
            elif self.state == RobotStateEnum.SEARCHING:
                try:
                    # self.handle_searching_state()
                    # self.get_logger().info("SEARCHING")
                    self.get_logger().debug("开始处理搜索状态")
                    self.handle_searching_state()
                    self.get_logger().info("SEARCHING")
                    self.get_logger().debug("完成处理搜索状态")
                except Exception as e:
                    self.get_logger().error(f"搜索状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.APPROACHING:
                try:
                    self.handle_approaching_state()
                except Exception as e:
                    self.get_logger().error(f"接近状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.ALIGNING:
                try:
                    self.handle_aligning_state()
                except Exception as e:
                    self.get_logger().error(f"对准状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.GRASPING:
                try:
                    self.handle_grasping_state()
                except Exception as e:
                    self.get_logger().error(f"抓取状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.TRANSPORTING:
                try:
                    self.handle_transporting_state(current_time)
                except Exception as e:
                    self.get_logger().error(f"运输状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.RELEASING:
                try:
                    self.handle_releasing_state()
                except Exception as e:
                    self.get_logger().error(f"释放状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.RETURNING:
                try:
                    self.handle_returning_state(current_time)
                except Exception as e:
                    self.get_logger().error(f"返回状态处理异常: {e}")
                    self.safe_fallback()
            
            elif self.state == RobotStateEnum.ERROR:
                try:
                    self.handle_error_state()
                except Exception as e:
                    self.get_logger().error(f"错误状态处理异常: {e}")
                    # 确保发送停止命令，避免无限循环
                    self.publish_cmd_vel(0.0, 0.0)
                    time.sleep(0.1)
            
            else:
                self.get_logger().error(f"未知状态: {self.state}")
                self.transition_to(RobotStateEnum.ERROR)
        
        except Exception as e:
            self.get_logger().error(f"状态机主循环异常: {e}")
            # 确保安全停止
            try:
                self.publish_cmd_vel(0.0, 0.0)
            except:
                pass
            self.transition_to(RobotStateEnum.ERROR)

    def safe_fallback(self):
        """安全回退策略"""
        # 停止所有运动
        self.publish_cmd_vel(0.0, 0.0)
        
        # 判断当前状态和持有球的情况
        if self.has_ball:
            # 如果已经抓取了球，尝试完成任务
            if self.state in [RobotStateEnum.TRANSPORTING, RobotStateEnum.RELEASING]:
                # 继续尝试运输或释放
                self.get_logger().info("尝试继续完成任务...")
            else:
                # 其他状态，尝试进入运输状态
                self.get_logger().info("检测到已抓取球体，尝试进入运输状态...")
                self.transition_to(RobotStateEnum.TRANSPORTING)
        else:
            # 如果没有球，回到搜索状态
            self.get_logger().info("回退到搜索状态...")
            self.transition_to(RobotStateEnum.SEARCHING)

    def check_hardware_status(self):
        """检查硬件状态"""
        # 实现硬件检查逻辑
        # 如检查最近消息时间戳，超时认为硬件有问题
        return True  # 简化示例，实际应检查硬件状态

    def check_sensor_status(self):
        """检查传感器状态"""
        # 实现传感器检查逻辑
        # 如检查相机是否提供最新图像
        return True  # 简化示例，实际应检查传感器状态

    def try_recover_system(self):
        """尝试恢复系统"""
        # 实现系统恢复逻辑
        # 如重新初始化硬件、重新连接传感器等
        self.get_logger().info("尝试恢复系统...")
        
        # 停止所有运动
        self.publish_cmd_vel(0.0, 0.0)
        
        # 尝试重置夹爪
        try:
            self.set_gripper(False)
        except:
            pass
        
        # 短暂延时
        time.sleep(0.1)

    # 状态处理函数
    def handle_searching_state(self):
        """处理搜索状态"""
        self.get_logger().debug("正在执行搜索状态处理")
        if self.current_ball is None:
            # 没有看到球，继续搜索
            self.get_logger().debug("未检测到球，继续搜索")
            self.publish_cmd_vel(0.0, self.search_angular_speed)
        else:
            # 找到球，转到接近状态
            self.get_logger().info(f"发现目标球: 位置=({self.current_ball.center.x}, {self.current_ball.center.y}), 直径={self.current_ball.diameter}")
            self.transition_to(RobotStateEnum.APPROACHING)

    def handle_approaching_state(self):
        """处理接近状态"""
        # 检查球是否丢失
        if self.is_ball_lost():
            self.get_logger().info("目标球丢失，回到搜索状态")
            self.transition_to(RobotStateEnum.SEARCHING)
            return

        # 计算球到中心的距离
        center_x = self.camera_width / 2 ##320
        x_error = self.current_ball.center.x - center_x #0
        
        # 判断是否足够接近
        if self.is_ball_close():
            self.transition_to(RobotStateEnum.ALIGNING)
            return
        
        # 导航到球位置
        self.approach_ball(x_error, center_x)

    def handle_aligning_state(self):
        """处理对准状态"""
        # 检查球是否丢失
        if self.is_ball_lost():
            self.get_logger().info("对准过程中目标球丢失，回到搜索状态")
            self.transition_to(RobotStateEnum.SEARCHING)
            return
        
        # 计算球到中心的距离
        center_x = self.camera_width / 2
        x_error = self.current_ball.center.x - center_x
        
        # 判断是否已经对准
        if abs(x_error) <= self.center_tolerance:
            self.get_logger().info("球已对准，准备抓取")
            self.publish_cmd_vel(0.0, 0.0)  # 停止
            self.transition_to(RobotStateEnum.GRASPING)
            return
        
        # 微调对准
        angular_speed = -0.05 * x_error / center_x
        self.publish_cmd_vel(0.0, angular_speed)

    def handle_grasping_state(self):
        """处理抓取状态"""
        # 执行抓取动作
        self.publish_cmd_vel(0.0, 0.0)  # 确保停止
        
        if self.set_gripper(True):  # 闭合夹爪
            self.get_logger().info("抓取成功")
            self.has_ball = True
            self.transition_to(RobotStateEnum.TRANSPORTING)
        else:
            self.get_logger().error("抓取失败")
            self.transition_to(RobotStateEnum.SEARCHING)

    def handle_transporting_state(self, current_time):
        """处理运输状态 - 使用视觉导航到目标区域紫色边框"""
        # 首次进入状态时初始化
        if not hasattr(self, 'transport_step'):
            self.transport_step = 1
            self.step_start_time = current_time
            self.target_search_dir = 1  # 1=顺时针, -1=逆时针
            self.search_duration = 0.0  # 搜索持续时间
            self.get_logger().info("开始运输: 寻找目标区域")
        
        # 根据当前步骤执行相应操作
        if self.transport_step == 1:
            # 步骤1: 旋转搜索目标区域
            found, target_pos, target_size = self.detect_target_zone()
            
            if found:
                # 找到目标区域，进入对准阶段
                self.get_logger().info(f"找到目标区域: 边界点位置=({target_pos[0]}, {target_pos[1]})")
                self.transport_step = 2
                self.step_start_time = current_time
                self.target_position = target_pos
                self.publish_cmd_vel(0.0, 0.0)  # 先停止
            else:
                # 未找到目标区域，继续旋转
                self.publish_cmd_vel(0.0, 0.3 * self.target_search_dir)  # 慢速旋转
                
                # 更新搜索持续时间
                self.search_duration = current_time - self.step_start_time
                
                # 如果搜索时间太长，改变搜索方向
                if self.search_duration > 8.0:
                    self.get_logger().warn("搜索目标区域超时，改变搜索方向")
                    self.target_search_dir *= -1  # 反转搜索方向
                    self.step_start_time = current_time  # 重置计时器
                
                # 如果在很长时间内仍未找到，尝试前进一点再搜索
                if self.search_duration > 16.0:
                    self.get_logger().warn("长时间未找到目标区域，前进一段距离再搜索")
                    self.publish_cmd_vel(0.15, 0.0)  # 前进
                    time.sleep(1.0)  # 前进一秒
                    self.publish_cmd_vel(0.0, 0.0)  # 停止
                    self.step_start_time = current_time  # 重置计时器
        
        elif self.transport_step == 2:
            # 步骤2: 对准目标边界点
            found, target_pos, target_size = self.detect_target_zone()
            
            if not found:
                # 目标区域丢失，回到搜索状态
                self.get_logger().warn("目标区域丢失，重新搜索")
                self.transport_step = 1
                self.step_start_time = current_time
                return
            
            # 更新目标位置
            self.target_position = target_pos
            
            # 计算目标区域偏移
            center_x = self.camera_width / 2
            x_error = self.target_position[0] - center_x
            
            # 检查是否对准
            if abs(x_error) <= self.center_tolerance:
                # 对准目标区域边界点，进入接近阶段
                self.get_logger().info("已对准目标区域边界点，开始接近")
                self.transport_step = 3
                self.step_start_time = current_time
                self.publish_cmd_vel(0.0, 0.0)  # 先停止
            else:
                # 旋转对准目标区域边界点
                angular_speed = -0.03 * x_error / center_x  # 减小系数使旋转更平滑
                self.publish_cmd_vel(0.0, angular_speed)
        
        elif self.transport_step == 3:
            # 步骤3: 接近目标边界点
            found, target_pos, target_size = self.detect_target_zone()
            
            if not found:
                # 目标区域丢失，尝试继续前进一小段时间
                if current_time - self.step_start_time < 1.0:
                    # 刚进入此阶段不久，可能是临时遮挡
                    self.get_logger().warn("接近过程中目标区域暂时丢失，继续前进")
                    self.publish_cmd_vel(0.1, 0.0)  # 低速前进
                else:
                    # 持续丢失，回到搜索状态
                    self.get_logger().warn("接近过程中目标区域持续丢失，重新搜索")
                    self.transport_step = 1
                    self.step_start_time = current_time
                return
            
            # 更新目标位置
            self.target_position = target_pos
            
            # 判断是否已足够接近
            # 使用边界点在图像中的位置判断
            # 当边界点靠近图像底部时，认为已经接近
            if self.target_position[1] > self.camera_height * 0.7:  # 在图像下部70%位置
                self.get_logger().info("已到达目标区域边界")
                self.transport_step = 4
                self.step_start_time = current_time
                self.publish_cmd_vel(0.0, 0.0)  # 停止
                return
            
            # 接近目标区域
            # 距离越近，速度越小
            y_position_ratio = self.target_position[1] / self.camera_height
            forward_speed = 0.15 * (1.0 - y_position_ratio * 0.7)  # 最大速度0.15，越近越慢
            
            # 同时保持对准
            center_x = self.camera_width / 2
            x_error = self.target_position[0] - center_x
            angular_speed = -0.02 * x_error / center_x  # 很小的修正
            
            self.publish_cmd_vel(forward_speed, angular_speed)
        
        elif self.transport_step == 4:
            # 步骤4: 抵达目标区域边界，前进一小段距离进入紫色区域
            if current_time - self.step_start_time < 1.0:  # 前进一秒钟进入区域
                self.publish_cmd_vel(0.1, 0.0)  # 低速前进进入紫色区域
            else:
                self.get_logger().info("已进入目标区域，准备释放")
                self.publish_cmd_vel(0.0, 0.0)  # 停止
                delattr(self, 'transport_step')
                self.transition_to(RobotStateEnum.RELEASING)
    def handle_releasing_state(self):
        """处理释放状态"""
        if self.set_gripper(False):  # 打开夹爪
            self.get_logger().info("释放成功")
            self.has_ball = False
            self.total_collected += 1
            self.get_logger().info(f"已收集球数量: {self.total_collected}")
            
            # 后退一段距离
            self.publish_cmd_vel(-0.1, 0.0)
            time.sleep(1.0)
            self.publish_cmd_vel(0.0, 0.0)
            
            self.transition_to(RobotStateEnum.RETURNING)
        else:
            self.get_logger().error("释放失败")
            self.transition_to(RobotStateEnum.ERROR)

    def handle_returning_state(self, current_time):
        """处理返回状态"""
        if not hasattr(self, 'return_start_time'):
            self.return_start_time = current_time
        
        if current_time - self.return_start_time < 3.0:
            self.publish_cmd_vel(0.0, 0.5)  # 原地旋转
        else:
            delattr(self, 'return_start_time')
            self.get_logger().info("返回搜索位置")
            self.transition_to(RobotStateEnum.SEARCHING)

    def handle_error_state(self):
        """处理错误状态"""
        # 错误状态：停止并重置
        self.publish_cmd_vel(0.0, 0.0)
        self.get_logger().error("进入错误状态，尝试恢复")
        
        # 尝试释放夹爪
        self.set_gripper(False)
        
        # 等待一段时间后重新开始
        time.sleep(1.0)
        
        # 重新开始搜索
        self.has_ball = False
        self.transition_to(RobotStateEnum.SEARCHING)
    ############################################################3
        
    
    def is_ball_lost(self):
        """检查球是否丢失"""
        try:
            current_time = self.get_clock().now().seconds_nanoseconds()[0]
            return (
                self.current_ball is None or
                current_time - self.last_ball_time > self.ball_lost_timeout
            )
        except Exception as e:
            self.get_logger().error(f"检查球是否丢失时出错: {str(e)}")
            return True  # 出错时认为球已丢失，安全策略
    
    def is_ball_close(self):
        """检查是否足够接近球"""
        if self.current_ball is None:
            return False
        
        # 使用球的直径作为距离估计
        # 当球看起来足够大时，认为已经接近
        return self.current_ball.diameter >= self.camera_width * self.approach_distance##(640*0.3=192)
    
    def approach_ball(self, x_error, center_x):
        """向球移动的方法"""
        # 计算线性和角速度
        angular_speed = -0.1 * x_error / center_x  # 角速度与偏移成正比
        linear_speed = self.approach_linear_speed
        
        # 如果角度偏差大，降低前进速度
        if abs(x_error) > self.center_tolerance * 2:
            linear_speed *= 0.5
        
        self.publish_cmd_vel(linear_speed, angular_speed)
  
    # 添加目标区域检测方法
    def detect_target_zone(self):
        """使用目标区域检测服务"""
        if not self.target_detection_client.service_is_ready():
            self.get_logger().warn("目标区域检测服务不可用")
            return False, None, None
        
        request = DetectTargetZone.Request()
        request.target_color = self.target_color
        
        # 发送请求
        future = self.target_detection_client.call_async(request)
        
        # 使用超时等待响应
        timeout = 1.0  # 1秒超时
        start_time = time.time()
        
        while not future.done() and time.time() - start_time < timeout:
            rclpy.spin_once(self, timeout_sec=0.1)
        
        if future.done():
            try:
                response = future.result()
                if response.success:
                    # 这里返回的是紫色边框上的边界点，不是中心点
                    border_point = (response.center.x, response.center.y)
                    size = response.size
                    self.get_logger().info(f"目标区域检测成功: 边界点={border_point}, 尺寸={size}")
                    return True, border_point, size
                else:
                    self.get_logger().warn(f"目标区域检测未找到结果: {response.message}")
                    return False, None, None
            except Exception as e:
                self.get_logger().error(f"目标区域检测服务调用出错: {e}")
                return False, None, None
        else:
            self.get_logger().error("目标区域检测服务调用超时")
            return False, None, None

    def set_gripper(self, grip, force=0.7):
        """控制夹爪"""
        if not self.gripper_client.service_is_ready():
            self.get_logger().warn("夹爪服务不可用")
            return False

        request = SetGripper.Request()
        request.grip = grip
        request.force = force

        future = self.gripper_client.call_async(request)
        future.add_done_callback(self.gripper_callback)
        return True  # 假设调用成功

    def gripper_callback(self, future):
        """夹爪服务回调"""
        try:
            response = future.result()
            if response.success:
                self.get_logger().info("夹爪操作成功")
            else:
                self.get_logger().error("夹爪操作失败")
        except Exception as e:
            self.get_logger().error(f"夹爪服务调用出错: {e}")

    def change_target_color(self, new_color):
        """更改目标颜色"""
        if not self.change_color_client.service_is_ready():
            self.get_logger().warn("颜色更改服务不可用")
            return False

        request = ChangeColor.Request()
        request.new_color = new_color

        future = self.change_color_client.call_async(request)
        future.add_done_callback(self.change_color_callback)
        return True  # 假设调用成功

    def change_color_callback(self, future):
        """颜色更改服务回调"""
        try:
            response = future.result()
            if response.success:
                self.target_color = response.current_color
                self.get_logger().info(f"目标颜色已更改为: {self.target_color}")
                # self.publish_target_zone_marker()
            else:
                self.get_logger().error("颜色更改失败")
        except Exception as e:
            self.get_logger().error(f"颜色更改服务调用出错: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = StrategyNode()
    
    # 创建执行器
    executor = MultiThreadedExecutor(num_threads=4)
    executor.add_node(node)
    
    try:
        # 等待几秒钟以便其他节点启动
        time.sleep(1.0)
        # 开始收集任务
        node.start()
        # 运行执行器
        executor.spin()

    except KeyboardInterrupt:
        pass
    finally:
        # 确保停止运动
        node.publish_cmd_vel(0.0, 0.0)
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()